#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <signal.h>
#include <stdlib.h>
#include <bcm2835.h>
#include "i2c_if.h"
#include "spi_if.h"
#include "pca9685.h"
#include "ps2.h"
#include "comm.h"


#define PCA9685_I2C_SLAVE_ADDR      0x40
#define PCA9685_OE_BCM_PIN_NUM      4
#define PCA9685_WORKING_FREQ        50
#define PCA9685_DIR_SERVO_CHAN      2
#define PCA9685_CAMERA_LEFT_RIGHT_SERVO_CHAN    1
#define PCA9685_CAMERA_UP_DOWN_SERVO_CHAN       0
#define PCA9685_DC_MOTOR_FORWARD_CHAN           3
#define PCA9685_DC_MOTOR_BACKWARD_CHAN          4

#define DIR_SERVO_PWM_DEADZONE  100
#define DIR_SERVO_PWM_DEFAULT   300
#define DIR_SERVO_PWM_MIN       140
#define DIR_SERVO_PWM_MAX       400
#define DIR_SERVO_PWM_STEP      2

#define CAMERA_LEFT_RIGHT_SERVO_PWM_DEADZONE  100
#define CAMERA_LEFT_RIGHT_SERVO_PWM_DEFAULT   300
#define CAMERA_LEFT_RIGHT_SERVO_PWM_MIN       120
#define CAMERA_LEFT_RIGHT_SERVO_PWM_MAX       550
#define CAMERA_LEFT_RIGHT_SERVO_PWM_STEP      4

#define CAMERA_UP_DOWN_SERVO_PWM_DEADZONE  100
#define CAMERA_UP_DOWN_SERVO_PWM_DEFAULT   300
#define CAMERA_UP_DOWN_SERVO_PWM_MIN       110
#define CAMERA_UP_DOWN_SERVO_PWM_MAX       390
#define CAMERA_UP_DOWN_SERVO_PWM_STEP      4

#define DC_MOTOR_PWM_DEADZONE  0
#define DC_MOTOR_PWM_DEFAULT   2048
#define DC_MOTOR_PWM_MIN       0
#define DC_MOTOR_PWM_MAX       4096
#define DC_MOTOR_PWM_STEP      100


#define PS2_RECV_CS         22

// #define PS2_USING_GPIO_SPI
#ifdef PS2_USING_GPIO_SPI
#define PS2_RECV_MOSI       10
#define PS2_RECV_MISO       9
#define PS2_RECV_CLK        11
#define GPIO_SPI_DELAY      10
#endif

typedef struct {
    Pca9685ObjHdl   pca9685;
    ServoObjHdl     dirServo;
    ServoObjHdl     cameraLeftRight;
    ServoObjHdl     cameraUpDown;
    DcMotorObjHdl   dcMotor;
    Ps2ObjHdl       ps2;
} SystemCtxStru;

static SystemCtxStru g_systemCtx;


static void SystemDelayUs(unsigned int us)
{
    bcm2835_delayMicroseconds(us);
}

static void SystemDelayMs(unsigned int ms)
{
    bcm2835_delay(ms);
}

static void DevicePca9685SelChip(int level)
{
    if (level) {
        bcm2835_gpio_write(PCA9685_OE_BCM_PIN_NUM, HIGH);
    } else {
        bcm2835_gpio_write(PCA9685_OE_BCM_PIN_NUM, LOW);
    }
}

static int DevicePca9685Init(void)
{
    SystemCtxStru *ctx = &g_systemCtx;
    Pca9685CbStru cfg;
    Pca9685ParamStru param;

    bcm2835_gpio_fsel(PCA9685_OE_BCM_PIN_NUM, BCM2835_GPIO_FSEL_OUTP);
    bcm2835_gpio_write(PCA9685_OE_BCM_PIN_NUM, HIGH);

    i2c_init(PCA9685_I2C_SLAVE_ADDR, 10000, BCM2835_I2C_CLOCK_DIVIDER_626);

    Pca9685RegisterMalloc(malloc, free);

    // enable pca9685
    DevicePca9685SelChip(false);

    cfg.selChipCb  = DevicePca9685SelChip;
    cfg.i2cReadCb  = i2c_read;
    cfg.i2cWriteCb = i2c_write;
    cfg.delayUsCb  = SystemDelayUs;
    cfg.delayMsCb  = SystemDelayMs;
    ctx->pca9685   = Pca9685Init(&cfg, PCA9685_WORKING_FREQ);
    app_log("pca9685 init done");

    param.deadZone = DIR_SERVO_PWM_DEADZONE;
    param.minPwm   = DIR_SERVO_PWM_MIN;
    param.maxPwm   = DIR_SERVO_PWM_MAX;
    param.pwm      = DIR_SERVO_PWM_DEFAULT;
    param.step     = DIR_SERVO_PWM_STEP;
    ctx->dirServo = ServoInit(ctx->pca9685, &param, PCA9685_DIR_SERVO_CHAN);
    app_log("dir servo init done");

    param.deadZone = CAMERA_LEFT_RIGHT_SERVO_PWM_DEADZONE;
    param.minPwm   = CAMERA_LEFT_RIGHT_SERVO_PWM_MIN;
    param.maxPwm   = CAMERA_LEFT_RIGHT_SERVO_PWM_MAX;
    param.pwm      = CAMERA_LEFT_RIGHT_SERVO_PWM_DEFAULT;
    param.step     = CAMERA_LEFT_RIGHT_SERVO_PWM_STEP;
    ctx->cameraLeftRight = ServoInit(ctx->pca9685, &param, PCA9685_CAMERA_LEFT_RIGHT_SERVO_CHAN);
    app_log("camera left right servo init done");

    param.deadZone = CAMERA_UP_DOWN_SERVO_PWM_DEADZONE;
    param.minPwm   = CAMERA_UP_DOWN_SERVO_PWM_MIN;
    param.maxPwm   = CAMERA_UP_DOWN_SERVO_PWM_MAX;
    param.pwm      = CAMERA_UP_DOWN_SERVO_PWM_DEFAULT;
    param.step     = CAMERA_UP_DOWN_SERVO_PWM_STEP;
    ctx->cameraUpDown = ServoInit(ctx->pca9685, &param, PCA9685_CAMERA_UP_DOWN_SERVO_CHAN);
    app_log("camera up down servo init done");

    param.deadZone = DC_MOTOR_PWM_DEADZONE;
    param.minPwm   = DC_MOTOR_PWM_MIN;
    param.maxPwm   = DC_MOTOR_PWM_MAX;
    param.pwm      = DC_MOTOR_PWM_DEFAULT;
    param.step     = DC_MOTOR_PWM_STEP;
    ctx->dcMotor = DcMotorInit(ctx->pca9685, &param, PCA9685_DC_MOTOR_FORWARD_CHAN,
        PCA9685_DC_MOTOR_BACKWARD_CHAN);
    app_log("dc motor init done");

    return 0;
}

#ifdef PS2_USING_GPIO_SPI
static unsigned char DevicePs2SpiTransfer(unsigned char val)
{
    unsigned char retval = 0;
    int i;

    bcm2835_gpio_write(PS2_RECV_CLK, HIGH);

    for (i = 0; i < 8; ++i) {
        if (val & 0x01) {
            bcm2835_gpio_write(PS2_RECV_MOSI, HIGH);
        } else {
            bcm2835_gpio_write(PS2_RECV_MOSI, LOW);
        }

        val >>= 1;

        SystemDelayUs(GPIO_SPI_DELAY);

        bcm2835_gpio_write(PS2_RECV_CLK, LOW);

        SystemDelayUs(GPIO_SPI_DELAY);

        retval >>= 1;

        if (bcm2835_gpio_lev(PS2_RECV_MISO) == HIGH) {
            retval |= 0x80;
        } else {
            retval &= ~0x80;
        }

        SystemDelayUs(GPIO_SPI_DELAY);

        bcm2835_gpio_write(PS2_RECV_CLK, HIGH);

        SystemDelayUs(GPIO_SPI_DELAY);
    }

    bcm2835_gpio_write(PS2_RECV_CLK, HIGH);

    return retval;
}

static int DevicePs2SpiTransferNb(unsigned char *tx, unsigned char *rx, int len)
{
    int i;

    for (i = 0; i < len; ++i) {
        rx[i] = DevicePs2SpiTransfer(tx[i]);
    }
    return len;
}
#else
// static unsigned char DevicePs2SpiTransfer(unsigned char val)
// {
//     return spi_transfer(val);
// }

static void DevicePs2SpiTransferNb(unsigned char *tx, unsigned char *rx, int len)
{
    spi_transfernb((char *)tx, (char *)rx, len);
}
#endif

static void DevicePs2SpiCsn(int level)
{
    if (level) {
        bcm2835_gpio_write(PS2_RECV_CS, HIGH);
    } else {
        bcm2835_gpio_write(PS2_RECV_CS, LOW);
    }
}

static int DevicePs2Init(void)
{
    SystemCtxStru *ctx = &g_systemCtx;
    Ps2CbStru cfg;

    bcm2835_gpio_fsel(PS2_RECV_CS, BCM2835_GPIO_FSEL_OUTP);
    bcm2835_gpio_write(PS2_RECV_CS, HIGH);

#ifdef PS2_USING_GPIO_SPI
    bcm2835_gpio_fsel(PS2_RECV_MOSI, BCM2835_GPIO_FSEL_OUTP);
    bcm2835_gpio_write(PS2_RECV_MOSI, LOW);
    bcm2835_gpio_set_pud(PS2_RECV_MOSI, BCM2835_GPIO_PUD_DOWN);

    bcm2835_gpio_fsel(PS2_RECV_MISO, BCM2835_GPIO_FSEL_INPT);
    bcm2835_gpio_set_pud(PS2_RECV_MISO, BCM2835_GPIO_PUD_UP);

    bcm2835_gpio_fsel(PS2_RECV_CLK, BCM2835_GPIO_FSEL_OUTP);
    bcm2835_gpio_write(PS2_RECV_CLK, HIGH);
#else
    if (spi_init(BCM2835_SPI_BIT_ORDER_LSBFIRST,
                 BCM2835_SPI_MODE2,
                 BCM2835_SPI_CLOCK_DIVIDER_4096) != 0)
    {
        app_log_err("spi init failed");
        return -1;
    }
#endif

    Ps2RegisterMalloc(malloc, free);

    cfg.selChipCb = DevicePs2SpiCsn;
    cfg.spiXferCb = DevicePs2SpiTransferNb;
    cfg.delayUsCb = SystemDelayUs;
    cfg.delayMsCb = SystemDelayMs;
    ctx->ps2 = Ps2Init(&cfg);
    app_log("ps2 %p init done", ctx->ps2);

    return 0;
}

static int DeviceInit(void)
{
    if (!bcm2835_init()) {
        app_log_err("bcm2835_init failed. Are you running as root??\n");
        return -1;
    }

    // PCA9685
    DevicePca9685Init();

    // PS2
    DevicePs2Init();

    return 0;
}

static void SystemExitHandler(int signo)
{
    SystemCtxStru *ctx = &g_systemCtx;

    //System Exit
    app_log("Program Stop");

    Ps2Deinit(ctx->ps2);
    DcMotorDeinit(ctx->dcMotor);
    ServoDeinit(ctx->cameraUpDown);
    ServoDeinit(ctx->cameraLeftRight);
    ServoDeinit(ctx->dirServo);
    Pca9685Deinit(ctx->pca9685);

    i2c_deinit();
    spi_deinit();
    bcm2835_close();

    exit(0);
}

void DevicePs2KeyWork(unsigned char key)
{
    SystemCtxStru *ctx = &g_systemCtx;

    switch (key) {
        case PS2_KEY_SELECT:
            break;
        case PS2_KEY_L3:
            break;
        case PS2_KEY_R3:
            break;
        case PS2_KEY_START:
            break;
        case PS2_KEY_UP:
            break;
        case PS2_KEY_RIGHT:
            break;
        case PS2_KEY_DOWN:
            break;
        case PS2_KEY_LEFT:
            break;
        case PS2_KEY_L2:
            break;
        case PS2_KEY_R2:
            DcMotorRunPwm(ctx->dcMotor, DC_MOTOR_RUNSTATE_FORWARD, 2048);
            break;
        case PS2_KEY_L1:
            break;
        case PS2_KEY_R1:
            DcMotorRunPwm(ctx->dcMotor, DC_MOTOR_RUNSTATE_BACKWARD, 2048);
            break;
        case PS2_KEY_TRIANGLE:
            break;
        case PS2_KEY_ROUNDNES:
            break;
        case PS2_KEY_X:
            break;
        case PS2_KEY_SQUARE:
            break;
        default:
            return;
    }
}

void DevicePs2JoystickWork(unsigned char joystick, unsigned char value)
{
    SystemCtxStru *ctx = &g_systemCtx;

    switch (joystick) {
        case PS2_JOYSTICK_LEFT_UP:
            break;
        case PS2_JOYSTICK_LEFT_DOWN:
            break;
        case PS2_JOYSTICK_LEFT_LEFT:
            if (value) {
                ServoSetPulse(ctx->dirServo, DIR_SERVO_PWM_MIN);
            }
            break;
        case PS2_JOYSTICK_LEFT_RIGHT:
            if (value) {
                ServoSetPulse(ctx->dirServo, DIR_SERVO_PWM_MAX);
            }
            break;
        case PS2_JOYSTICK_RIGHT_UP:
            if (value) {
                ServoIncreasePwm(ctx->cameraUpDown);
            }
            break;
        case PS2_JOYSTICK_RIGHT_DOWN:
            if (value) {
                ServoDecreasePwm(ctx->cameraUpDown);
            }
            break;
        case PS2_JOYSTICK_RIGHT_LEFT:
            if (value) {
                ServoIncreasePwm(ctx->cameraLeftRight);
            }
            break;
        case PS2_JOYSTICK_RIGHT_RIGHT:
            if (value) {
                ServoDecreasePwm(ctx->cameraLeftRight);
            }
            break;
    }
}

int main(int argc, char const *argv[])
{
    SystemCtxStru *ctx = &g_systemCtx;
    unsigned char joystick[PS2_JOYSTICK_MAX];
    unsigned char key[PS2_KEY_MAX];
    int ret, i;

    app_log("********************");
    app_log("SmartCar client new start.");
    app_log("********************");

    DeviceInit();

    // Exception handling:ctrl + c
    signal(SIGINT, SystemExitHandler);

    app_log("init done");

    while (1) {
        ret = Ps2GetAllState(ctx->ps2, key, joystick);
        // app_log("Ps2GetAllState ret %d", ret);
        if (ret == 0) {
            for (i = 0; i < PS2_KEY_MAX; ++i) {
                if (key[i]) {
                    Ps2ShowKey(i);
                    DevicePs2KeyWork(i);
                }
            }

            if (key[PS2_KEY_R2] == 0 && key[PS2_KEY_R1] == 0) {
                DcMotorRunPwm(ctx->dcMotor, DC_MOTOR_RUNSTATE_IDLE, 0);
            }

            for (i = 0; i < PS2_JOYSTICK_MAX; ++i) {
                Ps2ShowJoystick(i, joystick[i]);
                DevicePs2JoystickWork(i, joystick[i]);
            }

            if (joystick[PS2_JOYSTICK_LEFT_LEFT] == 0 && joystick[PS2_JOYSTICK_LEFT_RIGHT] == 0) {
                ServoSetPulse(ctx->dirServo, DIR_SERVO_PWM_DEFAULT);
            }
        }

        SystemDelayMs(100);
    }

    SystemExitHandler(0);
    app_log("exit");
    return 0;
}
